import threading, Queue, random, thread,time
from math import *
from numpy import *
from copy import *
from numpy.linalg import *
from wall import *
import math
import pdb
seterr(all='raise') # Set the numpy default error level to raise
# MATH CONVENTION
# For now, the convention of the coordinate system is the first dimension of a position/displacement is X, the second is Y (i.e. [x,y]).  The top left corner of the window is
# the world coordinate system origin, Y grows as you go down the window and X grows as you go right on the window.  Positive rotation occurs in a CCW direction.
# For coordinate systems relative to the agent, forward is +Y axis, back is -Y axis, right is +X axis, and left is -X axis.  Turning left is a positive angle, and turning
# right is a negative angle.  Thus, a change a positive change in angle in the world is both a CCW rotation and to the agent turning left.
# World heading of 0 is to the right, world heading of pi/2 is down (parallel to the +Y axis)

# Agent Coordinate System
# Agent's heading is the positive Y axis.  Given a displacement vector disp=[x_0,y_0], then the angle atan2(y_0,x_0) is from the positive X axis of the agent's coordinate system.

# The world model keeps track of position/velocity/acceleration of the agents, handles collisions with other agents, walls, etc.  The model is connected to by the agent to control it's desired position and headings in several different formats.  This is also used by the agent_controller to do the same tasks the agent would like to do and also enter a debug mode that allows for special actions.
class world_model(object):
	def __init__(self):
		# Object Specific Settings, all indexed by object_num
		self.objects={}
		self.walls={}
		self.o_callback_function = None
		self.w_callback_function = None
		self.min_wall_dist=1 # Within this distance and the object will trigger a wall event (be listed as a close wall)
	def set_callbacks(self,o_callback_function,w_callback_function):
		self.o_callback_function = o_callback_function
		self.w_callback_function = w_callback_function
	def new_object(self,object_num,cur_object):
		self.objects[object_num]=cur_object
	def new_wall(self,wall_num,cur_wall):
		self.walls[wall_num]=cur_wall
	def remove_wall(self,wall_num):
		del self.walls[wall_num]
	def remove_object(self,object_num):
		"""Remove all traces of a specified object number"""
		self.objects[object_num].die()
		del self.objects[object_num]
	def rel_object_locations(self,object_num,mode=0):
		"""This returns an angle and magnitude describing the relative heading and distance to each object from the provided object.  The angle is relative to the object's heading (heading = +Y axis = 0 degrees, objects to the right of the object are 0-pi and -pi-0 on the left)."""
		# Mode=0 Is all agents Mode=1 all enemies Mode=2 all friends
		result=[]
		pos=self.objects[object_num].pos
		object_heading=self.objects[object_num].heading

		for i in self.objects.iterkeys():
			if i == object_num:
				continue
			if self.objects[i].object_type < 2:
				continue
			if mode==1 and self.objects[i].object_type==self.objects[object_num].object_type: # Skip friends
				continue
			if mode==2 and self.objects[i].object_type != self.objects[object_num].object_type: # Skip foes
				continue
			cur_pos=self.objects[i].pos
			temp_diff=[pos[0]-cur_pos[0],pos[1]-cur_pos[1]]
			dist=sqrt(temp_diff[0]*temp_diff[0]+temp_diff[1]*temp_diff[1])
			heading=((atan2(temp_diff[1],temp_diff[0])-object_heading) % (2*pi))-pi
			result.append([heading,dist])
		return result
	def min_collision_dist_rel(self,object_num,relative_heading):
		return self.min_collision_dist(object_num,self.objects[object_num].heading+relative_heading)

	def min_collision_dist(self,object_num,angle):
		a=self.min_wall_collision_dist(object_num,angle)
		return a[0]
	# Finds the maximum distance an object can move in a given direction before colliding with a wall or another object
	def min_wall_collision_dist(self,object_num,angle):
		"""This function returns the maximum magnitude allowed in a direction from a position and what object it hits at that magnitude."""
		try:
			pos_0=array([self.objects[object_num].pos[0],self.objects[object_num].pos[1],1.0])
			pos_1=array([cos(angle),sin(angle),0.0])+pos_0
			object_line=cross(pos_0,pos_1)
		except FloatingPointError:
			pdb.set_trace()
		object_segment=tuple(self.objects[object_num].pos)+(pos_1[0],pos_1[1])
		min_distance=float(1e10000)
		close_walls=[]
		
		#self.debug_print_object_data(object_num)
		# Find the minimum distance to a wall collision in the specified direction
		# This isn't valid when we aren't using points, we need to find the
		# TODO Use the wall points to solve the complicated case
		for wall_i in self.walls:
			cur_wall=self.walls[wall_i]
			cur_wall=(float(cur_wall.points[0]),float(cur_wall.points[1]),float(cur_wall.points[2]),float(cur_wall.points[3]))
			wall_pos0=array([cur_wall[0],cur_wall[1],1.0])
			wall_pos1=array([cur_wall[2],cur_wall[3],1.0])
			wall_line=cross(wall_pos0,wall_pos1)
			collision_point=cross(object_line,wall_line)
			try:
				collision_point=collision_point/collision_point[2]
			except ArithmeticError:
				continue
			# Check to see if the collision point is on the wall, and if the point is in the direction we are going
			if self.point_on_segment(collision_point,cur_wall) and self.is_monotonic(pos_0[0:2],pos_1[0:2],collision_point[0:2]):
				# Find smallest distance of the edge of the circle to the wall
				wall_vec=wall_pos1[0:2]-wall_pos0[0:2]

				object_vec1=collision_point[0:2]-pos_0[0:2]
				object_vec2=collision_point[0:2]-pos_1[0:2]
				if dot(object_vec1,object_vec1) > dot(object_vec2,object_vec2):
					object_vec=object_vec1
				else:
					object_vec=object_vec2

				try:
					phi_angle=acos(dot(wall_vec,object_vec)/(norm(wall_vec)*norm(object_vec)))
				except ArithmeticError:
					pdb.set_trace()
					continue
				if phi_angle > pi/2:
					phi_angle=pi-phi_angle
				min_distance_to_wall=norm(pos_0[0:2]-collision_point[0:2])*sin(phi_angle)-self.objects[object_num].radius-wall.wall_width/2.0
				temp_min_dist=sin(phi_angle)*min_distance_to_wall
				if temp_min_dist < self.min_wall_dist:
					close_walls.append(wall_i)
				min_distance=min(temp_min_dist,min_distance)
				#print('MinDistToWall:%f CurMinDist:%f') % (min_distance_to_wall,min_distance)
		return float(min_distance),close_walls
	def overlapping_objects(self,object_num):
		result=[]
		for obj in self.objects:
			if obj==object_num:
				continue
			xdiff = self.objects[object_num].pos[0]-self.objects[obj].pos[0]
			ydiff = self.objects[object_num].pos[1]-self.objects[obj].pos[1]
			disp = sqrt(xdiff*xdiff + ydiff*ydiff)
			if disp <= self.objects[object_num].radius + self.objects[obj].radius:
				result.append(obj)
		return result
	def object_collisions(self,object_num,disp,collision_margin=1,rho_eps=.001):
		# This assumes the displacement is smaller than the max velocity
		result=[]
		disp=copy(disp)
		orig_disp=copy(disp)
		theta=atan2(disp[1],disp[0])
		# Go through the list of objects, and find the smallest displacement we can move before we collide with one of them, 
		# then return the list of objects that would collide if we moved a small amount past that value (out of the originally found objects)
		pos0=array(self.objects[object_num].pos)
		min_rho=float(1e1000)
		cost=cos(theta) # Precomputed constants
		costsqr=cost*cost
		sint=sin(theta)
		sintsqr=sint*sint
		for i in self.objects.iterkeys():
			if i==object_num:
				continue
			rad_sum=self.objects[i].radius+self.objects[object_num].radius
			
			pos1=array(self.objects[i].pos)
			if norm(pos1-pos0) - rad_sum - norm(orig_disp) > 0:
				continue
			rho_part1=cost*pos1[0]+pos1[1]*sint-pos0[0]*cost-sint*pos0[1]
			rho_part2_t=float(2*pos0[0]*cost*sint*pos0[1]-2*pos1[1]*sint*pos0[0]*cost+2*cost*pos1[0]*pos1[1]*sint-2*cost*pos1[0]*sint*pos0[1]+rad_sum**2+pos0[0]**2*costsqr+costsqr*pos1[0]**2+2*pos0[0]*pos1[0]-2*costsqr*pos1[0]*pos0[0]-pos0[0]**2-pos1[0]**2-pos0[1]**2*costsqr+2*pos0[1]*pos1[1]*costsqr-pos1[1]**2*costsqr)
			rho_disp=float(1e1000)
			if rho_part2_t > 0: # If the second part will be real
				rho_part2=math.sqrt(rho_part2_t)
				rho0=rho_part1+rho_part2
				rho1=rho_part1-rho_part2
				rho=None
				rho_disp0=(rho0-rho_eps)*array([cos(theta),sin(theta)])
				rho_disp1=(rho1-rho_eps)*array([cos(theta),sin(theta)])
				if 0<=rho0 and (rho1<0 or rho0<rho1) and norm((pos0+rho_disp0)-pos1)-rad_sum >0:
					rho=rho0
					min_rho=min(min_rho,rho0)
				elif 0<=rho1 and (rho0<0 or rho1<rho0) and norm((pos0+rho_disp1)-pos1)-rad_sum >0:
					rho=rho1
					min_rho=min(min_rho,rho1)
					if min_rho != float(1e1000):
						try:
							rho_disp=(max(min_rho-rho_eps,0.0))*array([cos(theta),sin(theta)])
						except FloatingPointError:
							pdb.set_trace()
				
				print('ColObj: %d rho0: %f disp0: %s rho1: %f disp1: %s rho: %s') % (i,rho_part1+rho_part2,str([(rho_part1+rho_part2)*cos(theta),(rho_part1+rho_part2)*sin(theta)]),rho_part1-rho_part2,str([(rho_part1-rho_part2)*cos(theta),(rho_part1-rho_part2)*sin(theta)]),str(rho))
		used_rho_disp=True
		try:
			rho_disp=(max(min_rho-rho_eps,0.0))*array([cos(theta),sin(theta)])
		except FloatingPointError:
			used_rho_disp=False
		
		if used_rho_disp and (min_rho != float(1e1000)) and (norm(rho_disp) < norm(orig_disp)):
			used_rho_disp=True
			col_disp=(min_rho+collision_margin)*array([cos(theta),sin(theta)])
		else:
			used_rho_disp=False
			col_disp=copy(orig_disp)
		#print('ColObjEnd: %d min_rho: %f used_rho: %s rho_disp: %s orig_disp: %s') %(i,min_rho,str(used_rho_disp),str([rho_disp[0],rho_disp[1]]),str([orig_disp[0],orig_disp[1]]))
		pos0p=pos0+col_disp
		result=[]
		# Go back through the objects and see which of them collide at the pos0p point
		for i in self.objects.iterkeys():
			if i == object_num:
				continue
			cur_pos=array(self.objects[i].pos)
			temp_diff=pos0p-cur_pos
			dist_sqr=dot(temp_diff,temp_diff)
			rad_sum=self.objects[object_num].radius + self.objects[i].radius
			if dist_sqr < rad_sum*rad_sum:
				result.append(i)
		disp=col_disp
		if used_rho_disp:
		   disp=rho_disp
		for i in self.objects.iterkeys():
			if i == object_num or self.objects[i].object_type==0 or self.objects[i].object_type==1: # Skip if it is us or a ball/bullet
				continue
			pos1=array(self.objects[i].pos)
			if norm((pos0+disp)-pos1)-rad_sum <0: # Last chance, if we are going to move where we will collide with an object, then don't move at all
				disp=array([0.0,0.0])
		return result,disp
	@staticmethod
	def is_monotonic(pnt0,pnt1,pnt2):
		"""Given 3 points (numpy arrays), this will return true if dist(pnt0,pnt2)>dist(pnt1,pnt2), this assumes colinearity."""
		temp_diff0=pnt2-pnt0
		temp_diff1=pnt2-pnt1
		return sum(temp_diff0*temp_diff0) > sum(temp_diff1*temp_diff1)
	@staticmethod
	def point_on_segment(point,line_seg): # NOTE This point must lie on the line passing through the line_seg or else...
		"""Given a point that intersects with the line that passes through the line segment, this will return true if it lies on the line segment"""
		eps=.00001
		if (line_seg[0]-eps<=point[0]<=line_seg[2]+eps or line_seg[0]+eps>=point[0]>=line_seg[2]-eps) and (line_seg[1]-eps<=point[1]<=line_seg[3]+eps or line_seg[1]+eps>=point[1]>=line_seg[3]-eps):
			return True
		return False
	# These are to be interfaced with by the objects
	def object_move_rel_world(self,object_num,disp):
		cur_object=self.objects[object_num]
		try:
			temp_disp_mag=sqrt(disp[0]*disp[0]+disp[1]*disp[1]) # Compute vector magnitude
		except OverflowError:
			temp_disp_mag=float('inf')
		orig_pos=copy(cur_object.pos)
		temp_angle=atan2(disp[1],disp[0])
		min_wall_collision_mag,close_walls=self.min_wall_collision_dist(object_num,temp_angle)
		temp_disp_mag=min([cur_object.max_velocity,temp_disp_mag])
		# The wall stopped us
		while 0<min_wall_collision_mag < temp_disp_mag:
			# Move right up until the wall collision
			
			disp[0]+=cos(temp_angle)*min_wall_collision_mag
			disp[1]+=sin(temp_angle)*min_wall_collision_mag
			collided_objects,disp = self.object_collisions(object_num,disp)
			cur_object.pos[0]+=disp[0]
			cur_object.pos[1]+=disp[1]
			#for obj in collided_objects:
			#	self.o_callback_function(object_num,obj)
			temp_disp_mag-=min_wall_collision_mag
			temp_disp_mag+=.1
			# Find our new angle
			if len(close_walls) == 1:
				#print('One close wall')
				"""close_wall=close_walls[0]
				cur_wall=self.walls[close_wall].points
				wall_pos0=array([cur_wall[0],cur_wall[1]])
				wall_pos1=array([cur_wall[2],cur_wall[3]])
				wall_vec=wall_pos0-wall_pos1
				obj_disp=array(disp)
				phi_angle=acos(dot(wall_vec,obj_disp)/(norm(wall_vec)*norm(obj_disp)))
				if phi_angle > pi/2:
					phi_angle=pi-phi_angle
				temp_angle=pi-phi_angle # Find the new angle of the agent's heading"""
				temp_angle=pi-temp_angle
			elif len(close_walls) >= 2:
				print('Many close walls')
				temp_angle=pi-temp_angle
			min_wall_collision_mag,close_walls=self.min_wall_collision_dist(object_num,temp_angle)
		temp_disp_mag=min([cur_object.max_velocity,temp_disp_mag,min_wall_collision_mag])
		disp[0]=cos(temp_angle)*temp_disp_mag
		disp[1]=sin(temp_angle)*temp_disp_mag

		collided_objects,disp = self.object_collisions(object_num,disp)
		#print('NewDisp'+str(disp))
		
		cur_object.pos[0]+=disp[0]
		cur_object.pos[1]+=disp[1]
		for obj in collided_objects:
			self.o_callback_function(object_num,obj)
		
		disp, close_walls=self.min_wall_collision_dist(object_num,temp_angle)
		self.w_callback_function(object_num,close_walls,disp)

	def object_move_rel_object(self,object_num,disp):
		"""This takes the object heading and displacement vector, both off of the +X axis, and computes the angle off of the object heading (heading-disp).
		Positive angles have a motion towards the left of the object."""
		cur_object=self.objects[object_num]
		try:
			temp_disp_mag=sqrt(disp[0]*disp[0]+disp[1]*disp[1]) # Compute vector magnitude
		except OverflowError:
			temp_disp_mag=float('inf')

		temp_angle=atan2(disp[1],disp[0])
		temp_disp_mag=min([cur_object.max_velocity,temp_disp_mag])
		disp[0]=sin(temp_angle-cur_object.heading)*temp_disp_mag # same as cos(self.object_heading[object_num]+(pi/2-temp_angle))*temp_disp_mag
		disp[1]=cos(temp_angle-cur_object.heading)*temp_disp_mag # same as sin(self.object_heading[object_num]+(pi/2-temp_angle))*temp_disp_mag
		self.object_move_rel_world(object_num,disp)
		
	def object_turn_rel(self,object_num,heading_diff):
		cur_object=self.objects[object_num]
		cur_object.heading+=max(min(heading_diff,cur_object.max_heading_diff),-cur_object.max_heading_diff)
		if cur_object.heading >= 0:
			cur_object.heading=cur_object.heading % (2*pi)
		else:
			cur_object.heading=cur_object.heading % (-2*pi)
	# These are to be used during debugging and by the game_model
	def debug_print_object_data(self,object_num):
		print('Object[%d] Position(%f,%f) Heading(%f)') %(object_num,self.objects[object_num].pos[0],self.objects[object_num].pos[1],self.objects[object_num].heading)
